#!/bin/bash

OUTPUT_TO_STDOUT=$1

# 节点名称（必须与节点中定义一致）
NODE_NAME="/ros_webrtc"
# 启动命令
LAUNCH_CMD="roslaunch ros_webrtc config.launch"
# 日志路径
LOG_FILE="/mnt/czx/webrtc_setup.log"
# PID 文件
PID_FILE="/tmp/ros_webrtc.pid"

echo "[WATCHDOG] Starting ROS watchdog..." | tee -a "$LOG_FILE"

#modify max fd num，because the program has fd leakage
ulimit -n 4096

while true; do
    # 判断节点是否存在
    if ! rosnode list 2>/dev/null | grep -q "$NODE_NAME"; then
        echo "[WATCHDOG] Node $NODE_NAME not running, restarting..." | tee -a "$LOG_FILE"

        # 如果 PID 文件存在，尝试杀掉之前的 roslaunch 进程
        if [ -f "$PID_FILE" ]; then
            OLD_PID=$(cat "$PID_FILE")
            if ps -p "$OLD_PID" > /dev/null 2>&1; then
                echo "[WATCHDOG] Killing old roslaunch process (PID $OLD_PID)" | tee -a "$LOG_FILE"
                kill -9 "$OLD_PID"
            fi
            rm -f "$PID_FILE"
        fi

        # 启动 roslaunch
        echo "[WATCHDOG] Launching: $LAUNCH_CMD" | tee -a "$LOG_FILE"
        cd /opt/ros/noetic/
        source setup.bash
        if [ "$OUTPUT_TO_STDOUT" == "1" ]; then
            $LAUNCH_CMD 
        else
            $LAUNCH_CMD >> "$LOG_FILE" 2>&1 &
        fi
        NEW_PID=$!
        echo "$NEW_PID" > "$PID_FILE"
        sleep 5
    fi

    sleep 1
done
